#include <iostream>
#include <cassert>
#include <chrono>
#include <thread>
#include <atomic>
#include <memory>
#include "write_blf.h"

#define WRITE_FRAME_COUNT 100000000
#define WRITE_TIME 10
#define CANFD_THREAD_CNT 9
#define FLEX_THREAD_CNT 2

uint64_t getPosixTime() {
    auto now = std::chrono::system_clock::now();
    return std::chrono::time_point_cast<std::chrono::microseconds>(now).time_since_epoch().count();
}

std::atomic<bool> is_start[9];
std::atomic<bool> is_start_flex[2];

void tfn_canfd(std::shared_ptr<Writeblf> wb,int idx) {
    while (is_start[idx]) {
        int cnt = 0;
        while(cnt++ < WRITE_FRAME_COUNT){
            CanData frame{};
            frame.channel = 1;
            frame.data_len = 64;
            frame.id = 0x55;
            frame.type_flag = CAN_EN_FD | CAN_EN_BRS | CAN_FRAME_DATA | CAN_ID_STD;//| CAN_FRAME_REMOTE; //CAN_ID_STD;
            frame.time = getPosixTime() ;
            for(int i = 0 ; i < frame.data_len ; i++) {
                frame.data[i] = 0x55;
            }
            auto start_time = getPosixTime();
            wb->BlfWriteCanfdData (frame);
            std::cout << " <" << getPosixTime() - start_time << ">" << std::endl;
            std::this_thread::sleep_for(std::chrono::microseconds(WRITE_TIME));
        }
        is_start[idx] = false;
        printf("end write can blf\n");
    }
}

void tfn_flex(std::shared_ptr<Writeblf> wb, int idx) {
    while (is_start_flex[idx]) {
        int cnt = 0;
        while(cnt++ < WRITE_FRAME_COUNT){
            FlexrayData frame{};
            frame.channel = 1;
            frame.channel_mask = 0x1;
            frame.message_id = 0x55;
            frame.message_name = "Flexray";
            frame.byte_count = 520;
            frame.data_bytes = 32;
            frame.cycle_number = 16;
            frame.tag = 5;
            frame.frame_flags = 0x2;
            frame.frame_crc = 0x0;
            frame.headerCrc1 = rand() % 2000;
            frame.headerCrc2 = 0;
            frame.time = getPosixTime();
            frame.data.clear();
            for (int i = 0 ; i < frame.data_bytes ; i++)
                frame.data.emplace_back(i * 3);
            auto start_time = getPosixTime();
            wb->BlfWriteFlexrayData(frame);
            std::cout << " <" << getPosixTime() - start_time << ">" << std::endl;
            std::this_thread::sleep_for(std::chrono::microseconds(WRITE_TIME));
        }
        is_start_flex[idx] = false;
        printf("end write flexray blf\n");
    }
}

int main() {
    std::shared_ptr<Writeblf> wb = std::make_shared<Writeblf>("data.blf");
    wb->StartWriteBlf();
    for (auto & i : is_start)
        i = true;
    for (auto & i : is_start_flex)
        i = true;

    std::thread tid_can[CANFD_THREAD_CNT];
    for(int i = 0 ; i < CANFD_THREAD_CNT ; i++)
        tid_can[i] = std::thread(tfn_canfd, wb, i);
    std::thread tid_flex[FLEX_THREAD_CNT];
    for(int i = 0 ; i < FLEX_THREAD_CNT ; i++)
        tid_flex[i] = std::thread(tfn_flex, wb, i);

    while (true) {
        int cnt = 0;
        for(int i = 0 ; i < CANFD_THREAD_CNT ; i++) {
            if(!is_start[i])
                cnt++;
        }
        for(int i = 0 ; i < FLEX_THREAD_CNT ; i++) {
            if(!is_start_flex[i])
                cnt++;
        }
        if (CANFD_THREAD_CNT + FLEX_THREAD_CNT == cnt)
            break;
    }
    wb->EndWriteBlf();
    std::cout << "end write blf" << std::endl;;
    for(int i = 0 ; i < CANFD_THREAD_CNT ; i++) {
        tid_can[i].join();
    }
    for (int i = 0 ; i < FLEX_THREAD_CNT ; i++) {
        tid_flex[i].join();
    }
    return 0;

}
